/*
 The contents of this file are subject to the Mozilla Public License
 Version 1.1 (the "License"); you may not use this file except in
 compliance with the License. You may obtain a copy of the License at
 http://www.mozilla.org/MPL/

 Software distributed under the License is distributed on an "AS IS"
 basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
 License for the specific language governing rights and limitations
 under the License.

 Alternatively, the contents of this file may be used under the terms
 of the GNU Lesser General Public license (the  "LGPL License"), in which case the
 provisions of LGPL License are applicable instead of those
 above.

 For feedback and questions about our Files and Projects please mail us,
 Janek Schäfer (foxblock) , foxblock_gmail_com
 Alexander Matthes (Ziz) , zizsdl_at_googlemail.com
*/

#include "Physic.hpp"

Physic::Physic(Level* level,NUMBER_TYPE gravity, NUMBER_TYPE friction, NUMBER_TYPE drag, NUMBER_TYPE minimalAcc) {
    this->level = level;
    this->gravity = gravity;
    this->friction = friction;
    this->drag = drag;
    this->minimalAcc = minimalAcc;
}

Physic::~Physic() {
}

BaseUnit* Physic::testCollision(NUMBER_TYPE x,NUMBER_TYPE y,bool mustBeSelectable) {
    std::vector<BaseUnit*>::iterator it;
    for (it = units.begin(); it != units.end(); ++it)
        if ((*it)->x+(*it)->getSprite()->w/2 > x && (*it)->x-(*it)->getSprite()->w/2 < x &&
            (*it)->y+(*it)->getSprite()->h/2 > y && (*it)->y-(*it)->getSprite()->h/2 < y)
            if (not mustBeSelectable || (*it)->selectable)
                return (*it);
    return NULL;
}

int Physic::getTile(NUMBER_TYPE x,NUMBER_TYPE y) {
    if (x < -0.5)
        return -1;
    if (y < -0.5)
        return -1;
    if (x >= NUMBER_TYPE(level->getWidth())-0.5)
        return -1;
    if (y >= NUMBER_TYPE(level->getHeight())-0.5)
        return -1;
    int px = int(x+0.5);
    int py = int(y+0.5);
    return px+py*level->getWidth();
}

int Physic::getTileHigh(NUMBER_TYPE y,NUMBER_TYPE h) {
    return (getTile(0,y)-getTile(0,y-h))/level->getWidth()+1;
}

bool Physic::isWithHighPossible(int tile,int h,int AND) {
    bool result = true;
    for (int i=tile;i>tile-h*level->getWidth();i-=level->getWidth()) {
        if (i < 0)
            continue;
        if (i >= level->getWidth() * level->getHeight())
            continue;
        if (level->collisionLayer[i] & AND)
            result = false;
        if (level->collisionLayer[i] & 16)
            last_was_dead = true;
    }
    return result;
}

int Physic::getTileWidth(NUMBER_TYPE x,NUMBER_TYPE w) {
    if (x<-0.5) {
        w = w+(x+0.5);
        x = -0.5;
    }
    int r_tile = getTile(x+w,0);
    int l_tile = getTile(x,0);
    return r_tile-l_tile+1;
}

bool Physic::isWithWidthPossible(int tile,int w,int AND) {
    bool result = true;
    for (int i=tile;i<tile+w;i++) {
        if (i < 0)
            continue;
        if (i/level->getWidth() != tile/level->getWidth())
            continue;
        if (i >= level->getWidth() * level->getHeight())
            continue;
        if (level->collisionLayer[i] & AND)
            result = false;
        if (level->collisionLayer[i] & 16)
            last_was_dead = true;
    }
    return result;
}

void Physic::update() {
    //first Movement
    std::vector<BaseUnit*>::iterator it;
    for (it = units.begin(); it != units.end(); ++it) {
        (*it)->oaccX = (*it)->accX;

        if (fabs((*it)->accX)<minimalAcc)
            (*it)->accX = 0;
        else
        if ((*it)->accX>0)
            (*it)->accX -= drag*(*it)->velX*(*it)->velX;
        else
        if ((*it)->accX<0)
            (*it)->accX += drag*(*it)->velX*(*it)->velX;

            (*it)->ovelX = (*it)->velX;

        (*it)->velX += (*it)->accX;
        if (fabs((*it)->velX)<friction)
            (*it)->velX = 0;
        else
        if ((*it)->velX>0)
            (*it)->velX -= friction;
        else
            (*it)->velX += friction;

        (*it)->oaccY = (*it)->accY;

        if ((*it)->gravity)
            (*it)->velY -= gravity;

        if ((*it)->accY>0)
            (*it)->accY -= drag*(*it)->velY*(*it)->velY;
        else
        if ((*it)->accY<0)
            (*it)->accY += drag*(*it)->velY*(*it)->velY;

            (*it)->ovelY = (*it)->velY;

        (*it)->velY += (*it)->accY;
        {
            (*it)->ox = (*it)->x;
            (*it)->oy = (*it)->y;
        }
        (*it)->x += (*it)->velX;
        (*it)->y += (*it)->velY;
        {
            (*it)->was_pushed.clear();
            (*it)->collision = false;
            (*it)->collision_with_environment = false;
            (*it)->collision_with_solid_unit = false;
            (*it)->collision_left = false;
            (*it)->collision_right = false;
            (*it)->collision_up = false;
            (*it)->collision_down = false;
            (*it)->upupup = false;
            (*it)->pushed_left = false;
            (*it)->pushed_right = false;
            (*it)->was_pushed_left = false;
            (*it)->was_pushed_right = false;
            (*it)->newVely = 0;
            (*it)->crushed_left = false;
            (*it)->crushed_right = false;
            (*it)->crushed_up = false;
            (*it)->crushed_down = false;
            (*it)->should_be_dead = false;
        }
    }

    std::vector<BaseUnit*>::iterator unitA;
    std::vector<BaseUnit*>::iterator unitB;
    bool found = true;
    #define LOOP_COUNTER 10
    int counter = LOOP_COUNTER;
    while (found)
    {
        if (counter <= 0)
        {
            printf("Bailed out of the first physics loop.\n");
            break;
        }
        --counter;
        found = false;
        for (unitA = units.begin(); unitA != units.end(); ++unitA) {
            for (unitB = units.begin(); unitB != units.end(); ++unitB) {
                if ((*unitA) == (*unitB))
                    continue;
                if (not (*unitB)->movable)
                    continue;
                if (not (*unitA)->solid)
                    continue;
                if (not (*unitB)->solid)
                    continue;
                //Collision for Movement
                if ((*unitA)->ox + (*unitA)->getSprite()->w/2 > (*unitB)->ox - (*unitB)->getSprite()->w/2 &&
                    (*unitA)->ox - (*unitA)->getSprite()->w/2 < (*unitB)->ox + (*unitB)->getSprite()->w/2 &&
                    (*unitA)->y + (*unitA)->getSprite()->h/2 + 0.001 > (*unitB)->y - (*unitB)->getSprite()->h/2 &&
                    (*unitA)->y - (*unitA)->getSprite()->h/2 - 0.001 < (*unitB)->y + (*unitB)->getSprite()->h/2) {
                    //the distance should be not to big:
                    //if (fabs((*unitB)->x-(*unitA)->x)<(*unitA)->getSprite()->w/4+(*unitB)->getSprite()->w/4)
                    if (fabs((*unitB)->x-(*unitA)->x)<(*unitA)->getSprite()->w/2+(*unitB)->getSprite()->w/2-0.2)
                    {
                        //Searching Unit A
                        std::vector<BaseUnit*>::iterator it;
                        for (it = (*unitB)->was_pushed.begin(); it != (*unitB)->was_pushed.end(); ++it)
                            if ((*it) == (*unitA))
                                break;
                        if (it == (*unitB)->was_pushed.end())
                        //is Unit B over Unit A?
                        if ((*unitB)->y > (*unitA)->y) {
                            //Unit B
                            if (fabs((*unitA)->x-(*unitA)->ox)>0.0001)
                            {
                              (*unitB)->was_pushed.push_back((*unitA));
                              (*unitB)->x += (*unitA)->x-(*unitA)->ox;
                              found = true;
                            }
                            if ((*unitA)->velY > 0 && (*unitA)->velY - (*unitB)->velY > 0 && (*unitA)->y < (*unitB)->y) {
                                //Unit B
                                found = true;
                                (*unitB)->y += (*unitA)->y-(*unitA)->oy;
                                (*unitB)->upupup = true;
                                if ((*unitA)->can_crush)
                                    (*unitB)->crushed_down = true;
                            }
                        }
                    }
                }
                if ((*unitA)->x + (*unitA)->getSprite()->w/2 > (*unitB)->x - (*unitB)->getSprite()->w/2 &&
                    (*unitA)->x - (*unitA)->getSprite()->w/2 < (*unitB)->x + (*unitB)->getSprite()->w/2 &&
                    (*unitA)->y + (*unitA)->getSprite()->h/2 - 0.001 > (*unitB)->oy - (*unitB)->getSprite()->h/2 &&
                    (*unitA)->y - (*unitA)->getSprite()->h/2 + 0.001 < (*unitB)->oy + (*unitB)->getSprite()->h/2) {
                    if (fabs((*unitA)->velY)<=0.001) {
                        //Unit As right side with Unit Bs left side
                        if (not (*unitA)->pushed_right && (*unitA)->velX > 0 && (*unitA)->velX - (*unitB)->velX > 0 && (*unitA)->x < (*unitB)->x) {
                            //Unit B
                            found = true;
                            if ((*unitA)->can_push)
                                (*unitB)->x += (*unitA)->x-(*unitA)->ox;
                            (*unitA)->velX*=0.98;
                            (*unitA)->pushed_right = true;
                            (*unitB)->was_pushed_left = true;
                            if ((*unitB)->collisionFeedBack)
                                (*unitB)->collisionFeedBack(*unitB,*unitA);
                            if ((*unitA)->collisionFeedBack)
                                (*unitA)->collisionFeedBack(*unitA,*unitB);
                        }
                        else
                        //Unit As right side with Unit Bs left side
                        if (not (*unitA)->pushed_left && (*unitA)->velX < 0 && (*unitA)->velX - (*unitB)->velX < 0 && (*unitA)->x > (*unitB)->x) {
                            //Unit B
                            found = true;
                            if ((*unitA)->can_push)
                                (*unitB)->x += (*unitA)->x-(*unitA)->ox;
                            (*unitA)->velX*=0.98;
                            (*unitA)->pushed_left = true;
                            (*unitB)->was_pushed_right = true;
                            if ((*unitB)->collisionFeedBack)
                                (*unitB)->collisionFeedBack(*unitB,*unitA);
                            if ((*unitA)->collisionFeedBack)
                                (*unitA)->collisionFeedBack(*unitA,*unitB);
                        }
                    }
                }
            }
        }
    }
    std::vector<BaseUnit*> unregisterAfterward;
    //Kollision mit Wand? => Position zurücksetzen & Beschleunigung = 0
    last_was_dead = false;
    for (it = units.begin(); it != units.end(); ++it) {
        float w = (*it)->getSprite()->w;
        float h = (*it)->getSprite()->h;
        //Down
        int AND = 2;
        if ((*it)->collision_on_everything && AND)
            AND = 15;
        if ((*it)->y-(*it)->oy<0.0 && not isWithWidthPossible(getTile((*it)->ox-w/2,-(*it)->y+h/2),getTileWidth((*it)->ox-w/2,w),AND)) {
            int newTile = getTile((*it)->ox,-(*it)->y+h/2.0);
            int oldTile = getTile((*it)->ox,-(*it)->oy+h/2.0);
            if (newTile!=oldTile) {
                (*it)->collision = true;
                (*it)->collision_with_environment = true;
                (*it)->y = (*it)->oy;
                (*it)->velY = 0;
                (*it)->accY = 0;
                (*it)->lastDown = true;
                (*it)->collision_down = true;
                (*it)->crushed_down = true;
            }
        }
        //Up
        AND = 1;
        if ((*it)->collision_on_everything && AND)
            AND = 15;
        if ((*it)->y-(*it)->oy>0.0 && not isWithWidthPossible(getTile((*it)->ox-w/2,-(*it)->y-h/2),getTileWidth((*it)->ox-w/2,w),AND)) {
            int newTile = getTile((*it)->ox,-(*it)->y-h/2);
            int oldTile = getTile((*it)->ox,-(*it)->oy-h/2);
            if (newTile!=oldTile) {
                (*it)->collision = true;
                (*it)->collision_with_environment = true;
                (*it)->y = (*it)->oy;
                (*it)->velY = 0;
                (*it)->accY = 0;
                (*it)->lastDown = false;
                (*it)->collision_up = true;
                (*it)->crushed_up = true;
            }
        }
        if ((*it)->collision == false)
            (*it)->lastDown = false;
        //Right
        AND = 8;
        if ((*it)->collision_on_everything && AND)
            AND = 15;
        if ((*it)->x-(*it)->ox>0 && not isWithHighPossible(getTile((*it)->x+w/2,-(*it)->y+h/2),getTileHigh(-(*it)->y+h/2,h),AND)) {
            (*it)->collision = true;
            (*it)->collision_with_environment = true;
            (*it)->collision_right = true;
            (*it)->crushed_right = true;
            (*it)->x = (*it)->ox;
            (*it)->velX = 0;
            (*it)->accX = 0;
        }
        //Left
        AND = 4;
        if ((*it)->collision_on_everything && AND)
            AND = 15;
        if ((*it)->x-(*it)->ox<0.0 && not isWithHighPossible(getTile((*it)->x-w/2,-(*it)->y+h/2),getTileHigh(-(*it)->y+h/2,h),AND)) {
            (*it)->collision = true;
            (*it)->collision_with_environment = true;
            (*it)->collision_left = true;
            (*it)->crushed_left = true;
            (*it)->x = (*it)->ox;
            (*it)->velX = 0;
            (*it)->accX = 0;
        }
        //Felt in the nothing:
        if ((*it)->y < - NUMBER_TYPE(level->getHeight()-1))
            last_was_dead = true;
        if (last_was_dead) {
            //printf("AH! The pain! IT HURTS!\n");
            unregisterAfterward.push_back((*it));
        }
        last_was_dead = false;
    }
    //Dead Units?
    for (it = unregisterAfterward.begin(); it != unregisterAfterward.end(); ++it) {
        if (not (*it)->undestroyable)
            unRegisterUnit((*it));
    }
    //Collision with other Units:
    found = true;
    counter = LOOP_COUNTER;
    while (found) {
        if (counter <= 0)
        {
            printf("Bailed out of the second physics loop.\n");
            break;
        }
        --counter;
        found = false;
        for (unitA = units.begin(); unitA != units.end(); ++unitA) {
            for (unitB = unitA+1; unitB != units.end(); ++unitB) {
                //Collision?
                if ((*unitA)->ox + (*unitA)->getSprite()->w/2 > (*unitB)->ox - (*unitB)->getSprite()->w/2 &&
                    (*unitA)->ox - (*unitA)->getSprite()->w/2 < (*unitB)->ox + (*unitB)->getSprite()->w/2 &&
                    (*unitA)->y + (*unitA)->getSprite()->h/2 > (*unitB)->y - (*unitB)->getSprite()->h/2 &&
                    (*unitA)->y - (*unitA)->getSprite()->h/2 < (*unitB)->y + (*unitB)->getSprite()->h/2) {
                    //Unit A Y with Unit B Y
                    if ((*unitA)->velY - (*unitB)->velY != 0) {
                        (*unitA)->collision = true;
                        (*unitB)->collision = true;
                        if ((*unitA)->collisionFeedBack)
                            (*unitA)->collisionFeedBack((*unitA),(*unitB));
                        if ((*unitB)->collisionFeedBack)
                            (*unitB)->collisionFeedBack((*unitB),(*unitA));
                        if ((*unitA)->solid && (*unitB)->solid) {
                            found = true;
                            //Unit A
                            if ((*unitA)->y<(*unitB)->y){
                                (*unitA)->collision_up = true;
                                if ((*unitB)->can_crush)
                                    (*unitA)->crushed_up = true;
                            }
                            else {
                                (*unitA)->collision_down = true;
                                if ((*unitB)->can_crush)
                                    (*unitA)->crushed_down = true;
                            }
                            (*unitA)->y = (*unitA)->oy;
                            (*unitA)->velY = 0;
                            (*unitA)->accY = 0;
                            (*unitA)->lastDown = (*unitA)->y >= (*unitB)->y;
                            (*unitA)->collision_with_solid_unit = true;
                            if ((*unitA)->lastDown)
                                (*unitA)->collision_down = true;
                            else
                                (*unitA)->collision_up = true;
                            //Unit B
                            if ((*unitB)->y<(*unitA)->y){
                                (*unitB)->collision_up = true;
                                if ((*unitA)->can_crush)
                                    (*unitB)->crushed_up = true;
                            }
                            else {
                                (*unitB)->collision_down = true;
                                if ((*unitA)->can_crush)
                                    (*unitB)->crushed_down = true;
                            }
                            (*unitB)->y = (*unitB)->oy;
                            (*unitB)->velY = 0;
                            (*unitB)->accY = 0;
                            (*unitB)->lastDown = (*unitA)->y < (*unitB)->y;
                            (*unitB)->collision_with_solid_unit = true;
                            if ((*unitB)->lastDown)
                                (*unitB)->collision_down = true;
                            else
                                (*unitB)->collision_up = true;
                        }
                    }
                }
                if ((*unitA)->x + (*unitA)->getSprite()->w/2 > (*unitB)->x - (*unitB)->getSprite()->w/2 &&
                    (*unitA)->x - (*unitA)->getSprite()->w/2 < (*unitB)->x + (*unitB)->getSprite()->w/2 &&
                    (*unitA)->y + (*unitA)->getSprite()->h/2 - 0.001 > (*unitB)->y - (*unitB)->getSprite()->h/2 &&
                    (*unitA)->y - (*unitA)->getSprite()->h/2 + 0.001< (*unitB)->y + (*unitB)->getSprite()->h/2) {
                    (*unitA)->collision = true;
                    (*unitB)->collision = true;
                    if ((*unitA)->collisionFeedBack)
                        (*unitA)->collisionFeedBack((*unitA),(*unitB));
                    if ((*unitB)->collisionFeedBack)
                        (*unitB)->collisionFeedBack((*unitB),(*unitA));
                    if ((*unitA)->solid && (*unitB)->solid) {
                        //Unit A X with Unit B X
                        if (((*unitA)->x-(*unitA)->ox) - ((*unitB)->x-(*unitB)->ox) != 0) {
                            found = true;
                            //Unit A
                            if ((*unitA)->x<(*unitB)->x){
                                (*unitA)->collision_right = true;
                                if ((*unitB)->can_crush)
                                    (*unitA)->crushed_right = true;
                            }
                            else {
                                (*unitA)->collision_left = true;
                                if ((*unitB)->can_crush)
                                    (*unitA)->crushed_left = true;
                            }
                            (*unitA)->x = (*unitA)->ox;
                            (*unitA)->velX = 0;
                            (*unitA)->accX = 0;
                            (*unitA)->collision_with_solid_unit = true;
                            //Unit B
                            if ((*unitB)->x<(*unitA)->x){
                                (*unitB)->collision_right = true;
                                if ((*unitA)->can_crush)
                                    (*unitB)->crushed_right = true;
                            }
                            else {
                                (*unitB)->collision_left = true;
                                if ((*unitA)->can_crush)
                                    (*unitB)->crushed_left = true;
                            }
                            (*unitB)->x = (*unitB)->ox;
                            (*unitB)->velX = 0;
                            (*unitB)->accX = 0;
                            (*unitB)->collision_with_solid_unit = true;
                        }
                    }
                }
                //Destroyed?
                if ((*unitB)->crushed_left && (*unitB)->crushed_right) {
                    unRegisterUnit((*unitB));
                    break;
                }
                if ((*unitB)->crushed_down && (*unitB)->crushed_up) {
                    unRegisterUnit((*unitB));
                    break;
                }
            }
            //Destroyed?
            if ((*unitA)->crushed_left && (*unitA)->crushed_right) {
                unRegisterUnit((*unitA));
                break;
            }
            if ((*unitA)->crushed_down && (*unitA)->crushed_up) {
                unRegisterUnit((*unitA));
                break;
            }
        }
    }
}

void Physic::registerUnit(BaseUnit* unit) {
    units.push_back(unit);
}

void Physic::unRegisterUnit(BaseUnit* unit) {
    std::vector<BaseUnit*>::iterator it;
    for (it = units.begin(); it != units.end(); ++it)
        if ((*it) == unit) {
            if ((*it)->unregisterFeedBack)
                (*it)->unregisterFeedBack(level,(*it));
            units.erase(it);
            break;
        }
}
